﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    ///
    /// System_OneLink
    ///
    public class System_OneLink : RoboticSystemComponent
    {
        /// ----------
        /// TODO:
        /// do not use static variables
        /// ----------
        private static double OneOverSix;
        private static double m1;
        private static double L1;
        private static double J1;
        private static double g;
        private static double De_tmp_inv;
        private static double W_tmp;
        private static double t_inc;
        private static double t_inc_Highspeed;
        private static double damping_factor;
        private static double[] q_0;
        private static double[] q_1;
        private static double[] q_2;
        private static double[] q_3;
        private static double[] k_1;
        private static double[] k_2;
        private static double[] k_3;
        private static double[] k_4;

        static public void InitStaticFields()
        {
            OneOverSix = 1 / 6.0;
            m1 = 1;
            g = 9.8;
            L1 = 1;
            damping_factor = 0;
            J1 = m1 * L1 * L1 / 12;
            De_tmp_inv = 1 / (J1 + m1 * L1 * L1 / 4);
            W_tmp = -m1 * g * L1 / 2;
            t_inc = 0.002;
            t_inc_Highspeed = 0.01;
            q_0 = new double[2];
            q_1 = new double[2];
            q_2 = new double[2];
            q_3 = new double[2];
            k_1 = new double[2];
            k_2 = new double[2];
            k_3 = new double[2];
            k_4 = new double[2];
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double torque = ctrl[0];
            qd[0] = q[1];
            qd[1] = (torque - m1 * g * L1 * Math.Cos(q[0]) * 0.5 - damping_factor * q[1]) * 3 / (m1 * L1 * L1);
        }

        private void Qd(double i_q, double i_qd, double i_c, ref double[] o_qdd)
        {
            o_qdd[0] = i_qd;
            o_qdd[1] = De_tmp_inv * (i_c + W_tmp * Math.Cos(i_q));
        }
        override public bool LoadingSystemParameters(string fileName)
        {
            base.LoadingSystemParameters(fileName);
            if (this.System_Type.Equals("Robotics_OneLink", StringComparison.OrdinalIgnoreCase) == false)
                throw new Exception("input file does not contain OneLink system");
            System_OneLink.InitStaticFields();
            return true;
        }

        override public bool Propagate(double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            int n_step = (int)Math.Floor(duration / t_delta + 0.5);
            double[] q_in = new double[q_start.Length];
            MotionPlanningUtils.vector_set(q_start, ref q_in);
            step_cost = 0;
            for (int i = 0; i < n_step; ++i)
            {
                this.Integration(q_in, u, t_delta, ref q_end);
                MotionPlanningUtils.vector_set(q_end, ref q_in);
                step_cost += t_delta;
            }

            while (q_end[0] < -Math.PI) q_end[0] += Math.PI + Math.PI;
            while (q_end[0] > Math.PI) q_end[0] -= Math.PI + Math.PI;

            return true;
        }


        public override bool Propagate_HighSpeed(double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            int n_step = (int)Math.Floor(duration / System_OneLink.t_inc_Highspeed + 0.5);

            q_0[0] = q_start[0];
            q_0[1] = q_start[1];

            for (int i = 0; i < n_step; ++i)
            {
                Qd(q_0[0], q_0[1], u[0], ref k_1);
                k_1[0] *= t_inc;
                k_1[1] *= t_inc;

                q_1[0] = q_0[0] + k_1[0] * 0.5;
                q_1[1] = q_0[1] + k_1[1] * 0.5;
                Qd(q_1[0], q_1[1], u[0], ref k_2);
                k_2[0] *= (t_inc + t_inc);
                k_2[1] *= (t_inc + t_inc);

                q_2[0] = q_0[0] + k_2[0] * 0.5;
                q_2[1] = q_0[1] + k_2[1] * 0.5;
                Qd(q_2[0], q_2[1], u[0], ref k_3);
                k_3[0] *= (t_inc + t_inc);
                k_3[1] *= (t_inc + t_inc);

                q_3[0] = q_0[0] + k_3[0];
                q_3[1] = q_0[1] + k_3[1];
                Qd(q_3[0], q_3[1], u[0], ref k_4);
                k_4[0] *= t_inc;
                k_4[1] *= t_inc;

                q_0[0] = q_0[0] + (k_1[0] + k_2[0]*2 + k_3[0]*2 + k_4[0]) * OneOverSix;
                q_0[1] = q_0[1] + (k_1[1] + k_2[1]*2 + k_3[1]*2 + k_4[1]) * OneOverSix;
            }

            while (q_0[0] < -Math.PI) q_0[0] += Math.PI + Math.PI;
            while (q_0[0] > Math.PI) q_0[0] -= Math.PI + Math.PI;

            q_end[0] = q_0[0];
            q_end[1] = q_0[1];

            step_cost = duration;

            return true;
        }

        public bool Propagate_old(double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            int n_step = (int)Math.Floor(duration / System_OneLink.t_inc + 0.5);

            q_0[0] = q_start[0];
            q_0[1] = q_start[1];

            for (int i = 0; i < n_step; ++i)
            {
                Qd(q_0[0], q_0[1], u[0], ref k_1);
                k_1[0] *= t_inc;
                k_1[1] *= t_inc;

                q_1[0] = q_0[0] + k_1[0] * 0.5;
                q_1[1] = q_0[1] + k_1[1] * 0.5;
                Qd(q_1[0], q_1[1], u[0], ref k_2);
                k_2[0] *= (t_inc + t_inc);
                k_2[1] *= (t_inc + t_inc);

                q_2[0] = q_0[0] + k_2[0] * 0.5;
                q_2[1] = q_0[1] + k_2[1] * 0.5;
                Qd(q_2[0], q_2[1], u[0], ref k_3);
                k_3[0] *= (t_inc + t_inc);
                k_3[1] *= (t_inc + t_inc);

                q_3[0] = q_0[0] + k_3[0];
                q_3[1] = q_0[1] + k_3[1];
                Qd(q_3[0], q_3[1], u[0], ref k_4);
                k_4[0] *= t_inc;
                k_4[1] *= t_inc;

                q_0[0] = q_0[0] + (k_1[0] + k_2[0] + k_3[0] + k_4[0]) * OneOverSix;
                q_0[1] = q_0[1] + (k_1[1] + k_2[1] + k_3[1] + k_4[1]) * OneOverSix;
            }

            while (q_0[0] < -Math.PI) q_0[0] += Math.PI + Math.PI;
            while (q_0[0] > Math.PI) q_0[0] -= Math.PI + Math.PI;

            q_end[0] = q_0[0];
            q_end[1] = q_0[1];

            step_cost = duration;

            return true;
        }
        public override double Distance2_Topological(double[] x1, double[] x2)
        {
            double diff_q1 = Math.Abs(x1[0] - x2[0]);
            while (diff_q1 > Math.PI)
                diff_q1 -= Math.PI;
            diff_q1 *= q_scale[0];
            double diff_q2 = (x1[1] - x2[1]) * q_scale[1];
            return diff_q1 * diff_q1 + diff_q2 * diff_q2;
        }
        public override string ToString()
        {
            string content;
            content = "\n\t<System_Type>Robotics_OneLink</System_Type>" +
                        base.ToString() +
                        "\t<Model_Parameters>" +
                        "\n\t\t<l1>" + L1 + "</l1>" +
                        "\n\t\t<m1>" + m1 + "</m1>" +
                        "\n\t\t<gravity>" + g + "</gravity>\n" +
                        "\n\t\t<damping_factor>" + damping_factor + "</damping_factor>\n" +
                        "\t</Model_Parameters>\n";
            return content;
        }

        override public bool Steering(double[] q_start, double[] q_end, ref double[] u, ref double duration, ref double edgeCost)
        {
            throw new NotImplementedException();
        }
    }
}
